#include "CMotionPlusCalibrateState.h"
#include "CStateMachine.h"
#include "../core/CRetroInput.h"
#include "../core/CDirect3D.h"

CMotionPlusCalibrateState::CMotionPlusCalibrateState(void)
{
}


CMotionPlusCalibrateState::~CMotionPlusCalibrateState(void)
{
}


void CMotionPlusCalibrateState::Enter()
{
	m_bFinished = false;
	m_nCurrentFrame = 0;
	m_CurState = CALIB_FACEDOWN;
	m_nCounter = 0;
	m_dwLastTime = GetTickCount();

	FinalMotionPlusData.x = 0.0f;
	FinalMotionPlusData.y = 0.0f;
	FinalMotionPlusData.z = 0.0f;
}

void CMotionPlusCalibrateState::Input(CRetroInput* pInput)
{
	if(pInput->WiiKeyReleased(wiimote::buttons::_B) && m_bFinished)
	{
		// pop us off so we can return the data
		CStateMachine::GetInstance()->PopState(); // TODO: delete the popped state
	}

	if(m_CurState == CALIB_READING)
	{
		ThisFramesReading = pInput->GetWiiMotionPlusDelta(false);
	}
}

void CMotionPlusCalibrateState::Update(float fDeltaTime)
{
	const float EPSILON = 0.01f;
	switch(m_CurState)
	{
	case CALIB_FACEDOWN:
		{
			// make sure the wii remote is sitting facedown
			m_szStatus = "Place the Wii remote on a still surface!";

			// make sure its been laying face down for secconds
			DWORD NowTime = GetTickCount();
			if(NowTime - m_dwLastTime > 5000)
			{
				m_dwLastTime = 0;
				m_CurState = CALIB_READING;
			}
		}
		break;
	case CALIB_READING:
		{
			// grab a few frames
			m_szStatus = "Reading... Please wait...";

			memcpy(&MotionPlusData[m_nCurrentFrame], &ThisFramesReading, sizeof(vec3<float>));

			m_nCurrentFrame++;
			if(m_nCurrentFrame >= NUM_FRAMES_TO_HOLD)
			{
				m_CurState = CALIB_CALCULATE;
				m_nCurrentFrame = 0;
			}
		}
		break;
	case CALIB_CALCULATE:
		{
			// get the average of the captured frames
			// Note: for an ASync feel, increment do one per frame
			m_szStatus = "Calculating calibration data...";

			FinalMotionPlusData.x += MotionPlusData[m_nCurrentFrame].x;
			FinalMotionPlusData.y += MotionPlusData[m_nCurrentFrame].y;
			FinalMotionPlusData.z += MotionPlusData[m_nCurrentFrame].z;

			m_nCurrentFrame++;
			if(m_nCurrentFrame == NUM_FRAMES_TO_HOLD)
			{
				FinalMotionPlusData.x /= NUM_FRAMES_TO_HOLD;
				FinalMotionPlusData.y /= NUM_FRAMES_TO_HOLD;
				FinalMotionPlusData.z /= NUM_FRAMES_TO_HOLD;

				m_CurState = CALIB_FINISHED;
				m_bFinished = true;
			}

		}
		break;
	case CALIB_FINISHED:
		{
			// flag that we are done calibrating
			m_szStatus = "Finished! Press B to continue...";
		}
	}
}

void CMotionPlusCalibrateState::Render(CDirect3D* pRender)
{
	pRender->DrawTextA(const_cast<char*>(m_szStatus.c_str()), 540, 580, 255, 255, 255);
}

void CMotionPlusCalibrateState::Exit()
{
	CRetroInput::GetInstance()->SetMotionPlusCalibrationOffset(FinalMotionPlusData);
}
